#include "stdafx.h"
#include "PlanningEngine.h"
#include "plannode.h"
#include "agent.h"
#include "goal.h"
#include "planningstructs.h"

using namespace BAA;



bool BAA::PlanningEngine::BuildPlanTree( Agent * agent, Goal *goal )
{
	// for a the goal
	// find any actions whose postcondition is the goal state.

		// not the same goal, destroy this plan
	if(mPlanTree)
		mPlanTree->~PlanTreeNode();

		// build new plan tree;
		mPlanTree = new PlanTreeNode();
		mPlanTree->parent = NULL; // define the root.


		return ResolveNode(goal->Precondition(), mPlanTree);

}

void BAA::PlanningEngine::Create()
{
	mPlanTree = NULL;
	mCurrentGoal = NULL;

}



bool BAA::PlanningEngine::ResolveNode( std::string postcondition, PlanTreeNode* node )
{
	// Get actions for postcondition
	// for each action, resolve any precondition
	ActionMultiMap::iterator it;
	std::pair<ActionMultiMap::iterator,ActionMultiMap::iterator> ret;
	ret = mActionMultiMap.equal_range(postcondition);

	// watch to see if an action succeeds in resolving the postcondition
	bool hasSuccesfulAction = false;


	// for each action with the postcondition
	for (it=ret.first; it!=ret.second; ++it)
	{
		// what action are we going to try to perform
		Action * action = (*it).second;
		action->PrepareConditions(); // setup parameters for conditions.

		// prepare node for insertion
		ActionNode * actionNode = new ActionNode();
		actionNode->action = action;
		actionNode->parent = node;

		// do we need to add this action to the  nodes possible actions
		// check to see if its preconditions are true.
		// if they are true or can be resolved, add to possible actions

		 bool hasFalsePrecondition = false;  // watch for preconditions that fail
		 PlanTreeNode * subTree = NULL;      // to hold a subtree for this action

		 Condition * precondition;
		 Conditions * preconditions = action->Preconditions();
		 for(Conditions::iterator condIter = preconditions->begin(); condIter != preconditions->end() || hasFalsePrecondition == true; ++condIter)
		 {
			 
			 precondition = (*condIter); // get precondition

			 // if the precondition is true
			 if(precondition->isTrue())
			 {
				 // nothing required, move onto next condition
				 continue;		 
			 }
			 else
			 {
				 // can we solve for the failed precondition
				 subTree = new PlanTreeNode();
				 // resolve child node.
				 if(ResolveNode(precondition->Name(), subTree))
				 {
					 // if node succeeded, add subtree to actionNode
					 subTree->parent =actionNode;
					 actionNode->subPlans.push_back(subTree);
					 
				 }
				 else
				 {
					 hasFalsePrecondition = true;
				 }
			 }
		 }// end for each condition

		 if(hasFalsePrecondition)
		 {
			 // destroy action and its children.
			 actionNode->~ActionNode();
		 }
		 else // we have an action // add to tree
		 {
			 hasSuccesfulAction = true;
			 node->PossibleActions.push_back(actionNode);
		 }


	}// end for each action


	return hasSuccesfulAction;

}

void BAA::PlanningEngine::AddAction( std::string postcondition, Action* action )
{
	if(action == NULL)
		return;

	// add action based on its post condition name
	mActionMultiMap.insert(ActionMultiMap::value_type(postcondition,action));
	
}

PlanTreeNode* BAA::PlanningEngine::Update( Agent* agent, Goal* goal)
{
	
	

	// create new plan if the goal has changed or the plan is empty;
	if(goal != mCurrentGoal || mPlan.empty())
	{
		mCurrentGoal = goal;
		// build new plan tree
		if(BuildPlanTree(agent, goal))
		{
			// we have a plan tree
			// find the best path
			
			mPlan.clear();

			float utility = BuildPlanAStar(mPlanTree,&mPlan, 4,1);			

			if(utility < 0)
			{
				// do something? Cope?

			}
		}
	}

	return mPlanTree;

}

void BAA::PlanningEngine::Initialize(Agent* parent)
{
	mParent = parent;

}


float BAA::PlanningEngine::BuildPlanAStar(PlanTreeNode* currentPlanTreeNode, std::vector<Action*>* plan, int maxLength, float careOfConseqence)
{
	// take plan with highest utility
	if(maxLength == 0)
		return 0.0f;

	std::vector<ActionNode*>* children = &(currentPlanTreeNode->PossibleActions);

	// if we have no children, this node has no solution.
	if(children->empty())
	{
		return 0.0f;
	}
	else
	{
		// there are children
		// select the one with the highest utility
		// add to plan
		float higestUtility = -999999.0f;
		ActionNode* bestChild;

		ActionNode * currentNode;
		float nodeUtility;

		// for each possible action

		for(unsigned int i = 0; i < children->size(); ++i)
		{
			currentNode = children->at(i);
			nodeUtility = currentNode->action->CalculateStateUtility() + 
						  careOfConseqence * mParent->AppraisalProcess()->CalculateUtilityOfAction(currentNode->action);
						  
						  
				

			// check to see if this utility is better then others
			if(nodeUtility > higestUtility)
			{
				higestUtility = nodeUtility;
				bestChild = currentNode;
			}
		}// end for

		// we know have the best action from the lot.
		// add it to plan tree

		plan->push_back(bestChild->action);

		// if the action has children, for each child node, create find the plan.
		// for each child

		float childUtility=0;
		for(unsigned int i = 0; i < bestChild->subPlans.size(); ++i)
		{
			childUtility = BuildPlanAStar(bestChild->subPlans[i], plan, maxLength-1, careOfConseqence);
		}

		if(bestChild->subPlans.size() != 0)
		{
			childUtility /= (float) bestChild->subPlans.size();
			return (higestUtility + childUtility)/2.0f;
		}
		
		return higestUtility;

	}// end else no children in node
}
